#pragma config(Hubs,  S1, HTMotor,  HTMotor,  none,     none)
#pragma config(Sensor, S1,     ,               sensorI2CMuxController)
#pragma config(Sensor, S2,     sonar2,         sensorSONAR)
#pragma config(Sensor, S3,     HTSPB,          sensorI2CCustom9V)
#pragma config(Motor,  motorA,          hopperNXT,     tmotorNXT, PIDControl, reversed, encoder)
#pragma config(Motor,  motorB,           ,             tmotorNXT, openLoop)
#pragma config(Motor,  motorC,           ,             tmotorNXT, openLoop)
#pragma config(Motor,  mtr_S1_C1_1,     left,          tmotorTetrix, PIDControl, reversed, encoder)
#pragma config(Motor,  mtr_S1_C1_2,     right,         tmotorTetrix, PIDControl, encoder)
#pragma config(Motor,  mtr_S1_C2_1,     hopper,        tmotorTetrix, openLoop, reversed)
#pragma config(Motor,  mtr_S1_C2_2,     fingers,       tmotorTetrix, PIDControl, reversed, encoder)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#define sonar4 0
#define servo1 0
#define servo2 0

// based on PSP-Nx-motor-control.c
// http://www.mindsensors.com/index.php?module=documents&JAS_DocumentManager_op=viewDocument&JAS_Document_id=13

#include "PSP-Nx-lib.h"
#include "geary-nomux-stuff.c"

//////////////////////////////////////////////////////////////////////////////
//
//      Globals
//
/////////////////////////////////////////////////////////////////////////////

const ubyte Addr = 0x02;
const tSensors SensorPort = S4;        // Connect PSPNX sensorto this port!!
int nLeftButton = 0;
int nRightButton = 0;
int nEnterButton = 0;
int nExitButton = 0;
const int DeadZone = 25;
int hopper_up, hopper_down, hopper_score;
int move_hopper_ok = false;

#define HOPPERTOLERANCE 10 // NXT encoder count slop around each position
#define FINGERTOLERANCE 10

// encoder positions
#define HOPPERDOWN 5
#define HOPPERUP 65
#define HOPPERSCORE 80
#define FINGERSUP 0
#define FINGERSDOWN 3000
#define FINGERUPSPEED - 50
#define FINGERDOWNSPEED 50
#define HOPPERUPSPEED 40
#define HOPPERDOWNSPEED -10

//////////////////////////////////////////////////////////////////////////////
//
//      Functions
//
/////////////////////////////////////////////////////////////////////////////

void MoveHopper (int hopperTarget) {
  while (abs(nMotorEncoder[hopperNXT]-hopperTarget) > HOPPERTOLERANCE) {
 	  bool moveUp = nMotorEncoder[hopperNXT] < hopperTarget;
 	  bool ledColor = moveUp ? GREEN : RED;
 	  int motorSpeed = moveUp ? HOPPERUPSPEED : HOPPERDOWNSPEED;
   	motor[hopper] = motorSpeed;
   	LightLed(ledColor);
  }
  motor[hopper] = 0;
  LightsOff();
}

void MoveFingersDown () {
	motor[fingers] = FINGERDOWNSPEED;
}
void MoveFingersUp () {
	motor[fingers] = FINGERUPSPEED;
}
void MoveFingersByEncoder (int fingerTarget) {
  while (abs(nMotorEncoder[fingers]-fingerTarget) > FINGERTOLERANCE) {
 	  bool moveDown = nMotorEncoder[fingers] < fingerTarget;
 	  bool ledColor = moveDown ? RED : GREEN;
 	  int motorSpeed = moveDown ? FINGERDOWNSPEED : FINGERUPSPEED;
   	motor[fingers] = motorSpeed;
   	LightLed(ledColor);
  }
  motor[fingers] = 0;
  LightsOff();
}

void TestPSPAnalog () {
  psp currState;
	PSP_ReadButtonState(SensorPort, Addr, currState);
	if ((int)currState.l_j_y == 99 && (int)currState.r_j_y == 99) {
	  	PlaySound(soundDownwardTones);
	  	StopMotors();
      wait1Msec(500);
      StopAllTasks();
    }
  }

//////////////////////////////////////////////////////////////////////////////
//
//      Hopper Task
//
/////////////////////////////////////////////////////////////////////////////
  //////////////////////////////////////////////////////////////////////////////
//
//      Main
//
/////////////////////////////////////////////////////////////////////////////

task HopperPosition () {
  while (true) {
	    wait1Msec (500);
      if (move_hopper_ok) {

		  // determine where to position hopper
		  if (hopper_down) {
        nxtDisplayTextLine(3,"Hopper down");
        MoveHopper (HOPPERDOWN);
      } else if (hopper_up) {
        nxtDisplayTextLine(3,"Hopper up");
        MoveHopper (HOPPERUP);
      } else if (hopper_score) {
        nxtDisplayTextLine(3,"Hopper score");
        MoveHopper (HOPPERSCORE);
      } else {
        // default is up
        nxtDisplayTextLine(3,"Hopper DEFAULT");
        MoveHopper (HOPPERUP);
      }

    } // ok to move hopper
}}

//////////////////////////////////////////////////////////////////////////////
//
//      Main
//
/////////////////////////////////////////////////////////////////////////////


task
main ()
{
  int powerLeft = 0;
  int powerRight = 0;
  int d_left_X, d_left_Y;
  int d_right_X, d_right_Y;
  int fingers_up, fingers_down, fingers_up_encoder, fingers_down_encoder;
  int trigger_pressed, left_joystick_click, right_joystick_click;
  int tank_drive_ok = false;
  int steer_drive_ok = false;
  int turbo_speed = false;
  psp currState;

  //
  // Note: program cannot be terminated if we hijack the 'exit' button. So there has to be an escape sequence
  //       that will return buttons to system control! We'll use a triple click
  //
  nNxtExitClicks = 3;                // Triple clicking EXIT button will terminate program
  //
  nI2CBytesReady[SensorPort] = 0;

  SensorType[SensorPort] = sensorI2CMuxController;
  wait10Msec (100);

	InitProtoboardLights();  LightsOff();
  nMotorEncoder[hopperNXT] = 0;  //clear the LEGO motor encoders

  TestPSPAnalog();

  StartTask (HopperPosition);

  while ( true )
    {
      wait1Msec (100);

      PSP_ReadButtonState(SensorPort, Addr, currState);

      // joysticks
      d_left_X = (int)currState.l_j_x;
      d_left_Y = (int)currState.l_j_y;
      d_right_X = (int)currState.r_j_x;
      d_right_Y = (int)currState.r_j_y;

      // left button pad
      hopper_down = !(bool)currState.d;
      hopper_up = !(bool)currState.c || !(bool)currState.a;
      hopper_score = !(bool)currState.b;

      // keep hopper disabled until lifted manually, then go on automatic
      if (hopper_up) { move_hopper_ok = true; }

      // right button pad
      fingers_up_encoder = !(bool)currState.triang;
      fingers_down_encoder = !(bool)currState.cross;
      fingers_up = !(bool)currState.square;
      fingers_down = !(bool)currState.circle;

      // any of the trigger buttons
      trigger_pressed = (!(bool)currState.l1) || (!(bool)currState.l2)
                     || (!(bool)currState.r1) || (!(bool)currState.r2);

      left_joystick_click = !(bool)currState.l_j_b ;
      right_joystick_click = !(bool)currState.r_j_b;

      // by default, do not enable driving, to avoid driving off a table
      if (left_joystick_click) { tank_drive_ok = true; steer_drive_ok = false; }
      if (right_joystick_click) { tank_drive_ok = false; steer_drive_ok = true; }

      // drive slowly unless turbo
      turbo_speed = trigger_pressed;

      /*
      * Determine power for each motor.
       * in doing so, take the Y component of left joystick
       * and X component of right joystick
      */

      // tank drive
      if (tank_drive_ok) {
	      powerLeft = abs(d_left_Y) > DeadZone ? d_left_Y : 0;
	      powerRight = abs(d_right_Y) > DeadZone ? d_right_Y : 0;
	      if (!turbo_speed) {	powerLeft /= 2; powerRight /= 2; }
	  	  nxtDisplayTextLine(1,"Left: %d", powerLeft);
  		  nxtDisplayTextLine(2,"Right: %d", powerRight);

  		// steering
  	  } else if (steer_drive_ok) {
  	    // distribute power proportionally based on x value
  	    int differential = abs(d_right_X) > DeadZone ? d_right_X : 0;
	      int totalPower = abs(d_right_Y) > DeadZone ? d_right_Y : 0;
	      if (!turbo_speed) {	totalPower /= 2; }
	      float proportion = abs(differential) / 100.0;  // extreme steer -> 1
	      powerLeft = differential > 0 ? totalPower * proportion : totalPower * (1.0 - proportion);
	      powerRight = differential < 0 ? totalPower * proportion : totalPower * (1.0 - proportion);
	  	  nxtDisplayTextLine(1,"Left: %d", powerLeft);
  		  nxtDisplayTextLine(2,"Right: %d", powerRight);

  		  // driving not enabled yet
	    } else {
		    nxtDisplayTextLine(1,"Click a joystick");
		    nxtDisplayTextLine(2,"to drive");
		  }

		  motor[left] = powerLeft;
		  motor[right] = powerRight;

		  // determine where to position fingers
		  if (fingers_down_encoder) {
        nxtDisplayTextLine(4,"E Fingers down");
        MoveFingersByEncoder(FINGERSDOWN);
      } else if (fingers_up_encoder) {
        nxtDisplayTextLine(4,"E Fingers up");
        MoveFingersByEncoder(FINGERSUP);
      } else if (fingers_up) {
        nxtDisplayTextLine(4,"Fingers up");
        MoveFingersUp();
      } else if (fingers_down) {
        nxtDisplayTextLine(4,"Fingers down");
        MoveFingersDown();
      } else {
        // default is not moving
        nxtDisplayTextLine(4,"Fingers DEFAULT");
        motor[fingers] = 0;
      }
      nxtDisplayTextLine(5,"Fingers Enc: %d", nMotorEncoder[fingers]);


    }

  StopAllTasks ();
}
